//
// Created by yu on 2023/10/14.
//

#include <vector>
#include <iostream>
#include <iomanip>
#include "lk-motor.h"
#include "CANFrame.h"

using namespace std;

void lkMotor::motorRun(uint32_t id)
{
    vector<string> datas = {"88", "00", "00", "00", "00", "00", "00", "00"};
    vector<unsigned char> serialData = CAN::CreateCanStandardFrame(id, datas);
    CANserialPort.write(serialData);
}

void lkMotor::motorStop(uint32_t id)
{
    vector<string> datas = {"81", "00", "00", "00", "00", "00", "00", "00"};
    CAN::CreateCanStandardFrame(id, datas);
    vector<unsigned char> serialData = CAN::CreateCanStandardFrame(id, datas);
    CANserialPort.write(serialData);
}

void lkMotor::motorOff(uint32_t id)
{
    vector<string> datas = {"80", "00", "00", "00", "00", "00", "00", "00"};
    CAN::CreateCanStandardFrame(id, datas);
    vector<unsigned char> serialData = CAN::CreateCanStandardFrame(id, datas);
    CANserialPort.write(serialData);
}

int lkMotor::SpeedControl(uint32_t id, uint32_t speed)
{
    uint8_t byte[4];
    byte[0] = static_cast<uint8_t> (speed & 0xFF);
    byte[1] = static_cast<uint8_t> ((speed >> 8) & 0xFF);
    byte[2] = static_cast<uint8_t> ((speed >> 16) & 0xFF);
    byte[3] = static_cast<uint8_t> ((speed >> 24) & 0xFF);
    vector<string> datas = {"A2", "00", "00", "00"};

    for (uint8_t i: byte)
    {
        ostringstream ss;
        ss << hex << setw(2) << setfill('0') << static_cast<int>(i);
        datas.push_back(ss.str());
    }

    vector<unsigned char> serialData = CAN::CreateCanStandardFrame(id, datas);
    CANserialPort.write(serialData);
    string readData = CANserialPort.read(CANserialPort.available());
    CAN::CANFrame frame = CAN::ReadStandardFrame(readData);
    uint16_t encoderLocation;
    encoderLocation = (frame.data[7] << 8) | frame.data[6];
    return encoderLocation;
}

void lkMotor::TorqueControl(uint32_t id, float torque)
{

    double torqueCurrent = torque / 0.6;
    int16_t torqueDataInt = torqueCurrent * 62.5;
//  uint16_t torqueDataUint = torqueDataInt + 32767;

    uint8_t byte[2];
    byte[0] = static_cast<uint8_t> (torqueDataInt & 0xFF);
    byte[1] = static_cast<uint8_t> ((torqueDataInt >> 8) & 0xFF);

    vector<string> datas = {"A1", "00", "00", "00"};

    for (uint8_t i: byte)
    {
        ostringstream ss;
        ss << hex << setw(2) << setfill('0') << static_cast<int>(i);
        datas.push_back(ss.str());
    }
    datas.emplace_back("00");
    datas.emplace_back("00");

    vector<unsigned char> serialData = CAN::CreateCanStandardFrame(id, datas);
    CANserialPort.write(serialData);
    string readData = CANserialPort.read(CANserialPort.available());
    CAN::CANFrame frame = CAN::ReadStandardFrame(readData);

}